package lego.jbv;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

import lejos.geom.Point;
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.robotics.localization.OdometryPoseProvider;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.NavigationListener;
import lejos.robotics.navigation.Navigator;
import lejos.robotics.navigation.Pose;
import lejos.robotics.navigation.Waypoint;
import lejos.robotics.pathfinding.Path;
import lejos.util.Delay;

public class robot {
	private static final double wheelDiameter = 5.51;
	private static final double trackWidth = 16.01;
	private static DifferentialPilot pilot;
	private static Navigator navigator;
	private static Path route;
	private static boolean blocked = false;
	
	public static void main(String[] args) {
		init();
		pilot = new DifferentialPilot(wheelDiameter, trackWidth, Motor.B, Motor.C);
		navigator = new Navigator(pilot);
		
		route = new Path();
		
		BumperControl bumperControl = new BumperControl(SensorPort.S1, SensorPort.S4);
		bumperControl.start();
		
		navigator.addNavigationListener(new NavigationListener() {
			
			@Override
			public void pathInterrupted(Waypoint waypoint, Pose pose, int sequence) {
				// TODO Auto-generated method stub
				
			}
			
			@Override
			public void pathComplete(Waypoint waypoint, Pose pose, int sequence) {
				// TODO Auto-generated method stub
				
			}
			
			@Override
			public void atWaypoint(Waypoint waypoint, Pose pose, int sequence) {
				blocked = false;
				
			}
		});
		
		
		while(true)
		{
			createRoute();
			LCD.drawString("Press any button", 0, 0);
			Button.waitForAnyPress();
			LCD.clear();
			LCD.drawString("Driving", 0, 0);
					
			Delay.msDelay(200);
			LCD.drawString("Path size", 0, 2);
			LCD.drawInt(route.size(), 10, 2);
			navigator.followPath(route);
			while(navigator.isMoving())
			{
				if(bumperControl.getStatus().isBlocked() && !blocked)
				{
					blocked = true;
					navigator.stop();
					
					Delay.msDelay(500);
					
					avoid(route.indexOf(navigator.getWaypoint()));
				}
			}
			navigator.clearPath();
			LCD.drawString("Path cleared", 0, 1);
			LCD.drawString("Path size", 0, 2);
			LCD.drawInt(route.size(), 10, 2);
			//System.exit(1);
		}
		
		
		
/* DifferentialPilot pilot = new DifferentialPilot(wheelDiameter, trackWidth, Motor.B, Motor.C);
		
		route = new ArrayList<>();
		route.add(new AnglePoint(0, 0, 0));
		route.add(new AnglePoint(200, 0, 135));
		route.add(new AnglePoint(100, 100, 135));
		route.add(new AnglePoint(100, -50, -116.56));
		route.add(new AnglePoint(0, 0, -153.43));
		
		LCD.drawString("Press any button", 0, 0);
		Button.waitForAnyPress();
		LCD.clear();
		LCD.drawString("Driving", 0, 0);
		
		//tetst
		
		Delay.msDelay(200);
		

		for (int i = 0; i < (route.size() - 1); i++) 
		{
			AnglePoint cpoint = route.get(i);
			AnglePoint npoint = route.get(i+1);
			double dist = AnglePoint.Distance(cpoint, npoint);
			int id = 9;
			pilot.rotate(cpoint.getAngle());
			pilot.travel(AnglePoint.Distance(cpoint, npoint));
		}

		
		while(true)
		{
		} */
	}
	
	private static void createRoute() {
		route.clear();
		//route.add(new Waypoint(0, 0));
		route.add(new Waypoint(40, 0));
		route.add(new Waypoint(20, 20));
		route.add(new Waypoint(20, -10));
		route.add(new Waypoint(0, 0));
	}
	
	private static void avoid(int index)
	{
		LCD.drawString("Stalled", 0, 3);
		
		Random r = new Random();
		double x = navigator.getPoseProvider().getPose().getLocation().getX();
		double y = navigator.getPoseProvider().getPose().getLocation().getY();
		int limitx = 10, limity = 10;
		if(x > 0)
			limitx = -30;
		if(y > 0)
			limity = -30;
			

		int randx = limitx + r.nextInt(20);
		int randy = limity + r.nextInt(20);
		
		Waypoint point = new Waypoint(x-randx, y-randy); 
				
		route.add(index, point);
		LCD.drawString("Av x:" + Float.toString(point.x) + " y:" + Float.toString(point.y), 0, 3);
		navigator.followPath();
		//while(navigator.isMoving());
		//LCD.drawString("          ", 0, 3);
	}
	
	private static void init()
    {
    	Button.ESCAPE.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button b) {
				LCD.clear();
			    System.exit(0);				
			}
			
			@Override
			public void buttonPressed(Button b) {
				// TODO Auto-generated method stub
				
			}
		});
    }
}
